/**
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
  * @file       vision_attacker_node.cpp
  * @brief      上位机弹道解算节点
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2024-5          prom-se         1. done(原作者是西安理工大学的仓库中的开源)
  *  V1.0.1     2024-9-3        ZikangXie       1. 修改为RV格式的节点构造，和本队伍的所需话题对齐
  *
  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
  */
// 官方库
#include <rclcpp/rclcpp.hpp>
#include <visualization_msgs/msg/marker.hpp>

#include "eigen3/Eigen/Dense"
// 从vision_interfaces加载消息类型
// 用于接收串口传来的机器人自身云台信息，进行对应的弹道解算
#include <vision_interfaces/msg/robot.hpp>
// 从自瞄部分的auto_aim_interfaces加载消息类型
#include "auto_aim_interfaces/msg/target.hpp"
// 从弹道解算部分加载头文件
#include "vision_attacker/outpost.hpp"
#include "vision_attacker/vision_attacker.hpp"
// // 从串口通信的pb_rm_interfaces中加载消息类型
// #include "rm_serial_driver2025/pb_rm_interfaces/msg/gimbal_cmd.hpp"

namespace vision_attacker
{
VisionAttacker::VisionAttacker(const rclcpp::NodeOptions & options)
: Node("vision_attacker", options)
{
  /*固有参数设置*/
  RCLCPP_INFO(this->get_logger(), "Start VisionAttacker!");
  lagTime = declare_parameter("trajectory.lag_time", 0.08);
  airK = declare_parameter("trajectory.air_k", 0.04);
  yawFix = declare_parameter("trajectory.yaw_fix", 0.0);
  pitchFix = declare_parameter("trajectory.pitch_fix", 0.0);
  carThreshold = declare_parameter("fire_ctrl.car_attack_threshold", 30.0);
  outpostThreshold = declare_parameter("fire_ctrl.outpost_attack_threshold", 5.0);
  thresholdFix = declare_parameter("fire_ctrl.threshold_fix", 0.0);
  robotPtr = std::make_unique<vision_interfaces::msg::Robot>();
  /*发布话题可视化话题*/
  markerPub = this->create_publisher<visualization_msgs::msg::Marker>("aiming_point", 10);
  aimMarkerPub = this->create_publisher<visualization_msgs::msg::Marker>("real_aiming_point", 10);
  /*发布瞄准信息带串口通信*/
  cmd_gimbal_joint_pub_ =
    create_publisher<sensor_msgs::msg::JointState>("cmd_gimbal_joint", rclcpp::QoS(10).reliable());

  cmd_shoot_pub_ =
    create_publisher<example_interfaces::msg::UInt8>("cmd_shoot", rclcpp::QoS(10).reliable());

  // 订阅机器人当前yaw，pitch（弧度值）
  joint_state_sub_ = create_subscription<sensor_msgs::msg::JointState>(
    "serial/gimbal_joint_state", rclcpp::SensorDataQoS(),
    std::bind(&VisionAttacker::robot_callback, this, std::placeholders::_1));
  // 订阅跟踪结果
  targetSub = this->create_subscription<auto_aim_interfaces::msg::Target>(
    "tracker/target", rclcpp::SensorDataQoS(),
    std::bind(&VisionAttacker::target_callback, this, std::placeholders::_1));
  // 定义并初始化了两个 visualization_msgs::msg::Marker，用于在ROS中可视化标记点。
  aimPoint.header.frame_id = "chassis";
  aimPoint.ns = "aiming_point";
  aimPoint.type = visualization_msgs::msg::Marker::SPHERE;
  aimPoint.action = visualization_msgs::msg::Marker::ADD;
  aimPoint.scale.x = aimPoint.scale.y = aimPoint.scale.z = 0.12;
  aimPoint.color.r = 1.0;
  aimPoint.color.g = 1.0;
  aimPoint.color.b = 1.0;
  aimPoint.color.a = 1.0;
  aimPoint.lifetime = rclcpp::Duration::from_seconds(0.1);

  realAimPoint.header.frame_id = "chassis";
  realAimPoint.ns = "real_aiming_point";
  realAimPoint.type = visualization_msgs::msg::Marker::SPHERE;
  realAimPoint.action = visualization_msgs::msg::Marker::ADD;
  realAimPoint.scale.x = realAimPoint.scale.y = realAimPoint.scale.z = 0.12;
  realAimPoint.color.r = 1.0;
  realAimPoint.color.g = 0.0;
  realAimPoint.color.b = 0.0;
  realAimPoint.color.a = 1.0;
  realAimPoint.lifetime = rclcpp::Duration::from_seconds(0.1);
}
/*将串口传来的机器人当前状况赋值给robotptr指针；如果赋值过程中发生异常，则记录下来*/
void VisionAttacker::robot_callback(const sensor_msgs::msg::JointState gimbal_position)
{
  try {
    // RCLCPP_INFO(
    //   this->get_logger(), "------------receive 'serial_driver/robotgimbal' Node msg.-------------");
    // 确保JointState中的position数组至少有2个元素
    if (gimbal_position.position.size() >= 2) {
      // gimbal_position[0]:pitch ; gimbal_position[1]:yaw
      robotPtr->self_pitch = gimbal_position.position[0];  // pitch
      robotPtr->self_yaw = gimbal_position.position[1];    // yaw
      

      // RCLCPP_INFO(
      //   this->get_logger(), "接收到机器人自身Yaw: %f", robotPtr->self_yaw);
    } else {
      RCLCPP_ERROR(
        this->get_logger(),
        "Position data is insufficient (should have at least 2 elements for yaw and pitch).");
    }
  } catch (std::exception & ex) {
    RCLCPP_ERROR(this->get_logger(), "获取机器人信息'serial_driver/robotgimbal'时发生错误.");
  }
}
/*
基础参数：
        lag_time                    # 预测延迟时间修正
        air_k                       # 空气阻力系数
        yaw_fix                     # 输出Yaw修正
        pitch_fix                   # 输出Pitch修正
        car_attack_threshold        # 地面机器人装甲板攻击角度阈值
        outpost_attack_threshold    # 前哨站装甲板攻击角度阈值
        threshold_fix               # 角度阈值修正
跟踪目标tracker/target的参数：
        std_msgs/Header header
        bool tracking
        string id
        int32 armors_num
        geometry_msgs/Point position
        geometry_msgs/Vector3 velocity
        float64 yaw
        float64 v_yaw
        float64 radius_1
        float64 radius_2
        float64 dz
*/
void VisionAttacker::target_callback(const auto_aim_interfaces::msg::Target target_msg)
{
  // RCLCPP_INFO(get_logger(), "----接收到跟踪结果，启动target_callback----");
  /*1.从参数服务器获取参数*/
  lagTime = get_parameter("trajectory.lag_time").as_double();
  // 空气阻力系数
  airK = get_parameter("trajectory.air_k").as_double();
  yawFix = get_parameter("trajectory.yaw_fix").as_double();
  pitchFix = get_parameter("trajectory.pitch_fix").as_double();
  carThreshold = get_parameter("fire_ctrl.car_attack_threshold").as_double();
  outpostThreshold = get_parameter("fire_ctrl.outpost_attack_threshold").as_double();
  thresholdFix = get_parameter("fire_ctrl.threshold_fix").as_double();
  // 弹丸速度取恒定的20.00m/s
  muzzleSpeed = 20.00;
  /*2.从跟踪结点接收到的消息target中获取目标位置和距离*/
  Eigen::Vector2d xy;
  xy << target_msg.position.x, target_msg.position.y;
  // 为后面计算发射角度做准备
  double yDis = target_msg.position.z;
  double xDis = xy.norm();
  double flyTime = 0;
  solveTrajectory(flyTime, xDis, yDis, lagTime, muzzleSpeed, 0.04);

  /*3.初始化瞄准消息robot_position and robot_shoot*/
  robot_position.name.resize(2);
  robot_position.position.resize(2);
  robot_position.name[0] = "gimbal_pitch_joint";
  robot_position.name[1] = "gimbal_yaw_joint";
  robot_position.position[0] = robotPtr->self_pitch;
  robot_position.position[1] = robotPtr->self_yaw;
  robot_shoot.data = 0;

  /*4.如果目标没进入跟踪状态*/
  if (!target_msg.tracking) {
    robot_shoot.data = 0;
    // 这里我真觉得没必要发布，甚至觉得在这里发布不好
    cmd_gimbal_joint_pub_->publish(robot_position);
    // RCLCPP_INFO(get_logger(), "----Sad!目标没有进入跟踪状态,无法进行弹道解算----");
    return;
  } else { /*5.如果目标在追踪状态*/
    // RCLCPP_INFO(get_logger(), "----Happy!目标进入跟踪状态，开始弹道解算----");

    // 断目标的 id 是否为 "outpost"，如果是，使用 outpostThreshold，否则使用 carThreshold 作为攻击角度的阈值
    double threshold = target_msg.id == "outpost" ? outpostThreshold : carThreshold;
    // 根据目标的id，选择outpost" 或 "car"对应的类对象
    std::shared_ptr<carStates> states = target_msg.id == "outpost"
                                          ? std::make_shared<outpostStates>()
                                          : std::make_shared<carStates>();
    // 更新一个车的状态；self_yaw （弧度值）
    states->update(target_msg, robotPtr->self_yaw);
    // 预测一个车的四个装甲板的状态位置
    std::vector<armorStates> armors = states->getPreArmor(flyTime);
    // 根据偏航角差进行排序
    std::sort(armors.begin(), armors.end(), [](const auto & armor1, const auto & armor2) {
      return abs(armor1.delta_yaw) < abs(armor2.delta_yaw);
    });
    // 检查 最优装甲板（armors[0]）的偏航角差是否在可攻击的范围内
    if (
      (-threshold + thresholdFix) < armors[0].delta_yaw &&
      armors[0].delta_yaw < (threshold + thresholdFix)) {
      /*计算瞄准角度和发射条件*/
      // 计算目标装甲板的瞄准的绝对角度yaw
      robot_position.position[1] = atan2(armors[0].y, armors[0].x);
      // 下面这部分角度转化的代码是不必要的，因为atan2函数返回的角度是-π到π之间的，不需要转化
      // robot_position.position[1] =
      //   robot_position.position[1] < 0          ? robot_position.position[1] + 2 * M_PI
      //   : robot_position.position[1] > 2 * M_PI ? robot_position.position[1] - 2 * M_PI
      //                                           : robot_position.position[1];
      // 计算目标装甲板的瞄准角度pitch
      xDis = std::sqrt(armors[0].x * armors[0].x + armors[0].y * armors[0].y);
      robot_position.position[0] =
        solveTrajectory(flyTime, xDis, armors[0].z, lagTime, muzzleSpeed, airK);
      // 判断是否满足开火条件
      // 如果目标装甲板的偏航角和俯仰角与机器人的偏航角和俯仰角之间的差距都小于 5 度，则可以开火。
      robot_shoot.data = abs(robot_position.position[1] - robotPtr->self_yaw) < 5.0 &&
                             abs(robot_position.position[0] - robotPtr->self_pitch) < 5.0
                           ? 1
                           : 0;
      // 更新可视化参数
      aimPoint.pose.position.x = armors[0].x;
      aimPoint.pose.position.y = armors[0].y;
      aimPoint.pose.position.z = armors[0].z;
      realAimPoint.pose.position.x =
        target_msg.position.x - target_msg.radius_1 * cos(robotPtr->self_yaw);
      realAimPoint.pose.position.y =
        target_msg.position.y - target_msg.radius_1 * sin(robotPtr->self_yaw);
      realAimPoint.pose.position.z = target_msg.position.z;
    } else {
      robot_shoot.data = 0;
      double x, y, z;
      x = target_msg.position.x - target_msg.radius_1 * cos(robotPtr->self_yaw);
      y = target_msg.position.y - target_msg.radius_1 * sin(robotPtr->self_yaw);
      z = target_msg.position.z;
      xDis = std::sqrt(x * x + y * y);
      robot_position.position[1] = atan2(y, x);
      // robot_position.position[1] =
      //   robot_position.position[1] < 0          ? robot_position.position[1] + 2 * M_PI
      //   : robot_position.position[1] > 2 * M_PI ? robot_position.position[1] - 2 * M_PI
      //                                           : robot_position.position[1];
      robot_position.position[0] = solveTrajectory(flyTime, xDis, z, lagTime, muzzleSpeed, airK);
    }
    // 发布可视化消息
    aimPoint.header.stamp = now();
    markerPub->publish(aimPoint);
    aimMarkerPub->publish(realAimPoint);
    // 发布瞄准消息到串口通信模块
    robot_position.position[1] += yawFix;
    robot_position.position[0] += pitchFix;
    cmd_gimbal_joint_pub_->publish(robot_position);
    cmd_shoot_pub_->publish(robot_shoot);
    RCLCPP_INFO(
      get_logger(),
      "---Congradulation!目标在跟踪状态下完成了弹道解算！\n aimYaw:%05.2f \n aimPitch:%05.2f ---",
      robot_position.position[1], robot_position.position[0]);
  }
}

/*函数的参数包括飞行时间的引用 flyTime，水平距离 x，垂直距离 z，延迟时间 lagTime，
初速度 muzzleSpeed 和空气阻力系数 air_k。*/
double VisionAttacker::solveTrajectory(
  double & flyTime, const double x, const double z, const double lagTime, const double muzzleSpeed,
  const double air_k)
{
  // RCLCPP_INFO(get_logger(), "---弹道解算函数solveTrajectory启动！---");
  const double gravity = 9.78;
  double theta;  //发射角度
  double time;
  double aimZ;           //目标高度
  double realZ;          //实际高度
  double targetZ = 0.0;  // 枪口高度
  // 先将枪口高度设置为目标高度，然后在迭代中不断调整。
  aimZ = z;
  // 使用20次循环来迭代计算发射角度和飞行时间，直到实际高度和目标高度之间的差值小于0.001
  for (size_t i = 0; i < 20; i++) {
    // atan2函数的作用是根据两个坐标值计算 方向角度
    theta = atan2(aimZ, x);
    // 根据子弹初速度、可噢计算出子弹飞行时间，本质是t=x/v
    time = (exp(air_k * x) - 1) / (air_k * muzzleSpeed * cos(theta));
    // 在时间time下，子弹在飞行过程中实际到达的垂直高度（实际高度realZ）
    realZ = muzzleSpeed * sin(theta) * time - gravity * (time * time) / 2;
    // 根据实际高度和目标高度之间的差值来调整下次迭代中计算弹道时使用的枪口高度
    aimZ = aimZ + (z - realZ);
    if (abs(realZ - z) < 0.001) {
      time += lagTime;
      flyTime = time;
      targetZ = aimZ;
      break;
    } else {
      continue;
    }
  }
  // 计算得到的发射角度
  double launch_angle = atan2(targetZ, x);
  // 输出计算得到的发射角度
  // RCLCPP_INFO(this->get_logger(), "弹道解算部分 计算出的发射pitch角: %f", launch_angle);
  // 输出计算更新出的飞行时间
  // RCLCPP_INFO(this->get_logger(), "弹道解算部分 计算出的弹丸飞行时间: %f", flyTime);
  // 返回计算得到的发射角度
  return -launch_angle;
}

VisionAttacker::~VisionAttacker() {}
}  // namespace vision_attacker

#include <rclcpp_components/register_node_macro.hpp>

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable
// when its library is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(vision_attacker::VisionAttacker)